#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include "../../Common/defines.h"

#include <stdio.h>
#include <iostream>
#include <iomanip>

#pragma comment(lib, "opencv_core242d")
#pragma comment(lib, "opencv_highgui242d")
#pragma comment(lib, "opencv_video242d")

using namespace cv;
using namespace std;

#define processSpeed(v, a)	if( abs(a) > 0.0 )			\
								if(a > 0) a -= 1.f;		\
								else	  a += 1.f; 	\
							v += a;						\

static inline void checkBorders(Mat & img, Point2f & p, float & speedX, float & speedY, float & accelX, float & accelY)
{
	if(p.x > img.cols - 10)
	{
		p.x = (float)(img.cols - 10);
		speedX = 0.0f;
		accelX = 0.0f;
	}
	
	if(p.x < 10)
	{
		p.x = 10;
		speedX = 0.0f;
		accelX = 0.0f;
	}

	if(p.y > img.rows - 10)
	{
		p.y =(float)(img.rows - 10);
		speedY = 0.0f;
		accelY = 0.0f;
	}

	if(p.y < 10) 
	{
		p.y = 10;
		speedY = 0.0f;
		accelY = 0.0f;
	}
}

static inline void drawCross(Mat & img, Point center, Scalar color, int d)
{
	line(	img, 
			Point( center.x - d, center.y - d ),
			Point( center.x + d, center.y + d ), 
			color, 2, CV_AA, 0);

	line(	img, 
			Point( center.x + d, center.y - d ),
			Point( center.x - d, center.y + d ), 
			color, 2, CV_AA, 0 );
}

static inline void plotVector(Mat & img, vector<float> & v, float scaleX = 1.0, float scaleY = 1.0, Scalar c = COLOR_GREEN, int tp = 0)
{
	if(tp == 0)
	{
		for(unsigned int k = 0; k < (v.size() - 1); k++)
		{
			int x1 = (int)(k * scaleX);
			int y1 = (int)(v[k] * scaleY);

			int x2 = (int)((k+1) * scaleX);
			int y2 = (int)(v[k+1] * scaleY);

			line(img, Point(x1, y1), Point(x2, y2), c);
		}
	}

	if(tp == 1)
	{
		for(unsigned int k = 0; k < (v.size() - 1); k++)
		{
			int x1 = (int)(k * scaleX);
			int y1 = (int)(v[k] * scaleY);
			drawCross(img, Point(x1, y1), c, 3);
		}
	}
}

static inline void plotMat(Mat & m, string name)
{
	cout << name << endl;

	for(int i = 0; i < m.rows; i++ )
	{
		for(int j = 0; j < m.cols; j++ )
		{
			cout << setiosflags(ios::fixed) << setprecision(3) << m.at<float>(i,j) << "\t";
		}

		cout << endl;
	}

	cout << endl;
	cout << endl;
}

float randomN(int stddev)
{
	Mat dest(1,1,CV_32F);
	randn(dest, 0, stddev);
	return dest.at<float>(0,0);
}

int main(int, char**)
{
	printf( "\nExamle of using OpenCV's Kalman filter.\n");

	/* Initialization */
	srand((int)time(NULL));
	KalmanFilter KF(4, 4, 0);

	float sampleTime  = 0.3f;

	Point2f currentPosition(400, 400);
	Point2f measuredPosition = currentPosition;
	Point2f estimatedPosition = currentPosition;

	// Transition Matrix A
	KF.transitionMatrix = *(Mat_<float>(4, 4) <<	1,	1,	0,	0,
													0,	1,	0,	0,
													0,	0,	1,  1,
													0,	0,	0,	1  );
	// Matrix u
	Mat u = Mat::zeros(4,1,CV_32F);

	// Matrix B
	KF.controlMatrix = Mat(4,4,CV_32F);
	setIdentity(KF.controlMatrix);

	// Matrix H
	setIdentity(KF.measurementMatrix);
	KF.measurementMatrix.at<float>(1,1) = 0.0f;
	KF.measurementMatrix.at<float>(3,3) = 0.0f;

	// Matrix R
	setIdentity(KF.measurementNoiseCov, Scalar::all(10000));
	KF.measurementNoiseCov.at<float>(1,1) = 0.0f;
	KF.measurementNoiseCov.at<float>(3,3) = 0.0f;

	// Matrix Q
	setIdentity(KF.processNoiseCov, Scalar::all(1e-1));

	// Matrix P -> posteriori error estimate covariance matrix
	// init value
	setIdentity(KF.errorCovPost, Scalar::all(0));

	// set startPosition
	KF.statePost = *(Mat_<float>(4, 1)  << 0, 0, 0, 0);

	unsigned int cnt = 0;
	int keyCode  = 0;
	int dispersion = 30;	// variance of simulated measure point

	float speedX = 0.0f;
	float speedY = 0.0f;

	float accelX = 0.0f;
	float accelY = 0.0f;
	float accelInc = 5.0f;
	
	bool bContinue = true;
	
	float fps = 25.0f;

	float distortion = 0.0f;
	float distortionVal = 350.0f;
	
	// set to true if you want to simulate some sporadic measurement distortion
	bool addPeriodicDistortion = false;

	float distortionInterval = 3.0f;					// Seconds
	float distortionDuration = 1.0f;					// Seconds
	float distortionCnt = distortionInterval + 0.1f;	// Seconds

	while(bContinue)
	{
		Mat diagramm = Mat::zeros(800, 800, CV_8UC3);

		switch(keyCode)
		{
			// UP
		case 2490368:	accelY -= accelInc;	break;
			// DOWN
		case 2621440:	accelY += accelInc;	break;
			// LEFT
		case 2424832:	accelX -= accelInc;	break;
			// RIGHT
		case 2555904:	accelX += accelInc;	break;
		
		case 27:	bContinue = false;	break;
		case ' ':	distortion = distortionVal;	break;

		}

		// Calculate real point position
		currentPosition.x = currentPosition.x + sampleTime * speedX;
		currentPosition.y = currentPosition.y + sampleTime * speedY;
		
		checkBorders(diagramm, currentPosition, speedX, speedY, accelX, accelY);

		// Generate measured point position
		measuredPosition.x = currentPosition.x + randomN(1) * dispersion;
		measuredPosition.y = currentPosition.y + randomN(1) * dispersion + distortion;

		// simulate distorted measurement
		if(distortionCnt > distortionInterval && addPeriodicDistortion)
		{
			measuredPosition.x += distortionVal;
			if( distortionCnt > (distortionInterval + distortionDuration) )
				distortionCnt = 0;
		}

		distortion = 0.0;
		
		// estimation (prediction)
		KF.predict(u);

		// generate measurement
		Mat measurement = *(Mat_<float>(4, 1) <<  measuredPosition.x, 0,  measuredPosition.y, 0);

		// propagation (correction)
		KF.correct(measurement);
		estimatedPosition.x = (estimatedPosition.x * 0 + KF.statePost.at<float>(0,0)) / 1;
		estimatedPosition.y = (estimatedPosition.y * 0 + KF.statePost.at<float>(2,0)) / 1;
		
		// plot points
		drawCross(diagramm, currentPosition, COLOR_GREEN, 5);
		drawCross(diagramm, measuredPosition, COLOR_RED, 5);
		drawCross(diagramm, estimatedPosition, COLOR_YELLOW, 5);
		
		line(diagramm, currentPosition, measuredPosition, COLOR_RED, 2);
		line(diagramm, currentPosition, estimatedPosition, COLOR_TUERKIS, 2);

		/* Plot infos */
		drawCross(diagramm, Point(10, 10), COLOR_GREEN, 5);
		putText(diagramm, "Real position", Point(20, 15), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);

		drawCross(diagramm, Point(10, 30), COLOR_RED, 5);
		putText(diagramm, "Measured position", Point(20, 35), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);
		
		drawCross(diagramm, Point(10, 50), COLOR_YELLOW, 5);
		putText(diagramm, "Filtered position", Point(20, 55), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);


		imshow("diagram", diagramm);
		keyCode = waitKey((int)(1000/fps));

		processSpeed(speedX, accelX);
		processSpeed(speedY, accelY);
		
		distortionCnt += 1.0f/fps;
	}

	float xScale = 12.0f;
	float yScale = 12.0f;
	
	// plot values
	/*plotVector(diagramm, realValues, xScale, yScale, COLOR_GREEN);
	plotVector(diagramm, measuredValues, xScale, yScale, COLOR_RED, 1);
	plotVector(diagramm, filteredValues, xScale, yScale, COLOR_TUERKIS);*/

	// flip(diagramm ,diagramm, 0);
	return 0;
}